import java.io.IOException;

/**
 * 
 * @author Remi Destigny
 * 
 * Main class, instantiate the robot and create the path. Two path tracking algorithms are proposed : 
 * - Follow the carrot
 * - Pure pursuit
 */
public class Lokarria {

	/**
	 * Entry point, create the robot object and the path object before starting the path tracking algorithm
	 * Output the time it took to the robot to go through the whole path
	 * @param args Command line arguments 
	 */
	public static void main(String[] args) {
		Path p = null;
		Robot r = new Robot();
		r.getLaserProperties();
		if (args.length < 5) {
			System.out.println("Incorrect command line argument");
			printUsage();
			System.exit(0);
		} 
		else {
			try {
				p = new Path(args[0]);
			}
			catch (IOException e) {
				System.out.println("Unable to open file " + args[0]);
				System.exit(0);
			}
			if (args[1].equals("carrot")) {
				if (args.length < 7) {
					System.out.println("Not enough argument for follow the carrot algorithm");
					printUsageCarrot();
					System.exit(0);
				}
				else {
					double startTime = System.currentTimeMillis();
					followTheCarrot(r, p, Double.valueOf(args[2]), Double.valueOf(args[3]), 
							Double.valueOf(args[4]), Double.valueOf(args[5]), Double.valueOf(args[6]));
					double endTime = System.currentTimeMillis();
					System.out.println("Total time : " + (endTime - startTime) / 1000 + " secondes ");
				}
			}
			else if (args[1].equals("pursuit")) {
				double startTime = System.currentTimeMillis();
				purePursuit(r, p, Double.valueOf(args[2]), Double.valueOf(args[3]), Double.valueOf(args[4]));
				double endTime = System.currentTimeMillis();
				System.out.println("Total time : " + (endTime - startTime) / 1000 + " secondes ");
			}
			else {
				System.out.println("Unknow algorithm " + args[1]);
				printUsage();
				System.exit(0);
			}
		}
	}

	private static void printUsageCarrot() {
		System.out.println("lokarria pathFile carrot lookahead epsilonAngle epsilonDistance rotateSpeed linearSpeed");
	}

	private static void printUsagePursuit() {
		System.out.println("lokarria pathFile pursuit lookahead epsilonDistance linearSpeed");
	}
	
	private static void printUsage() {
		System.out.println("Robot path tracking algorithm example.\nUsage :");
		printUsageCarrot();
		printUsagePursuit();
		System.out.println("Remember to start MRDS first !");
	}

	public static void followTheCarrot(Robot r, Path p, double lookahead, 
			double epsilonAngle, double epsilonDistance, double rotateSpeed,
			double linearSpeed) {
		r.setConstantsForFollowTheCarrot(lookahead, epsilonAngle,
				epsilonDistance, rotateSpeed, linearSpeed);
		r.followPathWithFollowTheCarrot(p);
	}

	public static void purePursuit(Robot r, Path p, double lookahead, 
			double epsilonDistance, double linearSpeed) {
		r.setContantsForPurePursuit(lookahead, epsilonDistance, linearSpeed);
		r.followPahtWithPurePursuit(p);
	}
}
